
#ifndef _FULL_COVER_PLANNER_H_
#define _FULL_COVER_PLANNER_H_

#include "preprocess/map_process.h"
#include "coord_convert.h"
#include "polygon2d.h"
#include "cell_decomposer.h"
#include "loop_planner.h"
#include "bstphdns_planner.h"
#include "PathPlan.h"

#include<geometry_msgs/Polygon.h>

using namespace global_path_planner::preprocess;

namespace global_path_planner {
namespace ccpp {

class BstphdnsPlanner;
class LoopPlanner;

class FullCoverPlanner{
public:

    enum E_PATHTYPE
    {
        E_IMPLEMENT = 0,          // 推行路径
        E_BOUSTROPHEDON = 1,      // 弓字形
        E_LOOP = 2,               // 回字形
        E_EDGE = 3,               // 贴边
        E_EDGE_BOUSTROPHEDON = 4, // 贴边 + 弓字形
        E_EDGE_LOOP = 5,          // 贴边 + 回字形
        E_AUTO = 10               // 自适应
    };

    explicit FullCoverPlanner(float& radius, std::string& map_dirct, float& path_width,
                            std::shared_ptr<MapProcess>& map_proc,
                            std::shared_ptr<::common::planning::PathPlan>& path_plan_ptr);

    virtual ~FullCoverPlanner();

    // 更新地图
    void UpdateParam(std::string map_name,
                    std::string map_dir,
                    double origin_x,
                    double origin_y,
                    double resolution,
                    double area,
                    float path_width,
                    int16_t gen_area);

    // 求解路径
    std::map<uint8_t, std::vector<std::vector<std::pair<float, float>>>>
        SolvedPath( int8_t& offset, int16_t& clean_mode,
                    geometry_msgs::Pose& start, bool& region_inflate,
                    std::vector<geometry_msgs::Polygon>& all_polys,
                    std::pair<bool, std::tuple<float, float, float>>& docker_pose,
                    float& all_area, std::string& error_info);

private:
    // 初始化参数
    void InitParam();

    // 普通区域路径求解
    std::map<uint8_t, std::vector<std::vector<std::pair<float, float>>>>
        NormalAreaSolvedPath(Point_2& p_start, std::pair<bool, Point_2>& p_docker,
                            bool& region_inflate, int16_t& clean_mode, bool& loop,
                            std::vector<geometry_msgs::Polygon>& all_polys,
                            float& radius, float& offset_dist,
                            float& all_area, std::string& error_info);

    // 子任务间衔接
    std::vector<std::vector<std::pair<float, float>>>
        SubtasksLink(std::vector<std::vector<std::pair<float, float>>>& origin_paths);

    // 距离当前点最近的轮廓索引
    std::pair<int, Point_2>
        IndexOfMinDistToContours(Point_2& p_start, std::map<int, std::pair<cv::Mat,std::vector<std::vector<cv::Point>>>>& contours);

    // std::vector<std::vector<std::pair<float, float>>>
    //     NormalAreaSolvedPath(Point_2& p_start, std::pair<bool, Point_2>& p_docker,
    //                         bool& region_inflate, int16_t& clean_mode, bool& loop,
    //                         std::vector<geometry_msgs::Polygon>& all_polys,
    //                         float& offset_dist, float& radius, std::string& error_info);

    // 功能区域路径求解
    std::map<uint8_t, std::vector<std::vector<std::pair<float, float>>>>
        FuncAreasSolvedPath();

    // 获取可通行区域
    std::map<int, std::pair<cv::Mat,std::vector<std::vector<cv::Point>>>>
        getAccessAreas(Point_2& p_start, bool& loop,
                    std::pair<bool, Point_2>& p_docker,
                    cv::Mat& img, cv::Mat& erode_img,
                    std::string& error_info);

    // 依据起点或充电桩位置是否有连通区域
    std::map<int, std::pair<cv::Mat,std::vector<std::vector<cv::Point>>>>
        hasAccessContours(cv::Point2f& default_pose, bool flag, cv::Mat& img,
                        std::vector<std::vector<cv::Point>>& cnts,
                        std::map<int, std::pair<cv::Mat,std::vector<std::vector<cv::Point>>>>& all_contours);
    
    /**
     * @brief 对离散点路径pair_points进行平滑
     */
    bool SmoothPairPoints(std::vector<std::pair<float, float>>& pair_points) const;

    /**
     * @brief 对离散点路径pair_points进行平滑并处理碰撞风险路点
     */
    bool SmoothAndProcessRiskPairPoints(std::vector<std::pair<float, float>>& pair_points) const;

private:
    // 地图处理类
    std::shared_ptr<MapProcess> __map_Proc_Ptr;
    // // 路径生成类
    // PathGeneration* __path_Gen;
    // 坐标转换
    CoordConvert __coord_convert;
    // 弓字形规划
    std::shared_ptr<BstphdnsPlanner> __bstphns_Planner_Ptr;
    // 回字形/贴边规划
    std::shared_ptr<LoopPlanner> __loop_Planner_Ptr;
    // 多边形模型
    std::shared_ptr<Polygon2D> __polygon2d_Ptr;
    // 单元分解
    std::shared_ptr<CellDecomposer> __cell_Decomp_Ptr;
    // p2p规划
    std::shared_ptr<::common::planning::PathPlan> __path_Plan_Ptr;

private:
    // 路径宽度
    float __path_Width;
    // // 动态路径规划偏移距离
    // float __offset_Dist;
    // 动态偏移
    // uint8_t __offset;
    // 路径之间的距离
    float __path_interval;
    // 机器人半径
    float __robot_Radius;
    // 扩展半径
    float __extend_Radius;
    // 地图文件夹
    std::string __map_Direct;
    // 地图名
    std::string __map_Name;
    // 地图原始坐标
    double __map_OriginX;
    double __map_OriginY;
    // // 地图宽度和高度
    // uint32_t __map_Width;
    // uint32_t __map_Height;
    // 地图分辨率
    float __map_Resolution;
    // 贴边距离
    float __perim_Dist;
    // 是否为圆弧弓字形
    bool __arc_Boustrophedon;
    // 回字形是否平滑
    bool __loop_Smooth;
    // 是否包含圆形
    bool __contain_Circle;
    // 是否只生成规划区域面积
    int16_t __gen_Area;
    // 子任务间是否需要链接
    bool __subtasks_Link;

};

}  // namespace ccpp
}  // namespace global_path_planner

#endif  // _FULL_COVER_PLANNER_H_